/**
rs_d455设备
*/


#include <librealsense2/rs.hpp>
#include <iostream>

#include "rs_common_device.h"

// opencv
#include <opencv2/opencv.hpp>



class RsD455Device: public rsCmmonDevice {

public:
  using Ptr = std::shared_ptr<RsD455Device>;

  static constexpr int IMU_RATE = 200;
  static constexpr int NUM_CAMS = 2;        // 相机数量
  int frame_num = 0;
  int imu_num = 0;
  RsD455Device(bool manual_exposure, int skip_frames, int webp_quality, double exposure_value = 10.0);

  // 开启设备
  void start();
  // 关闭设备
  void stop();
  // 设置曝光
  bool setExposure(double exposure);  // in milliseconds
  // 设置跳过的frames
  void setSkipFrames(int skip);
  
  void setWebpQuality(int quality);

 private:

  // realsense的一些配置
  rs2::context context;
  rs2::config config;
  rs2::pipeline pipe;
  rs2::sensor sensor;
  rs2::pipeline_profile profile;
};



std::string get_date();


RsD455Device::RsD455Device(bool manual_exposure, 
                           int skip_frames,
                           int webp_quality, 
                           double exposure_value):
      rsCmmonDevice("rsD455"){       

  std::cout << "open realsense d455" << std::endl;
  
  rsCmmonDevice::manual_exposure = manual_exposure;
  rsCmmonDevice::skip_frames = skip_frames;
  rsCmmonDevice::webp_quality = webp_quality;

  rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);
  pipe = rs2::pipeline(context);

  config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);// 使能加速度
  config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F); // 使能角速度
	config.enable_stream(RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 30);// left IR
	config.enable_stream(RS2_STREAM_INFRARED, 2, 640, 480, RS2_FORMAT_Y8, 30);// right IR

  // config.disable_stream(RS2_STREAM_DEPTH);

  if (context.query_devices().size() == 0) {
    std::cout << "Waiting for device to be connected" << std::endl;
    rs2::device_hub hub(context);
    hub.wait_for_device();
  }

  for (auto& s : context.query_devices()[0].query_sensors()) {
    std::cout << "Sensor " << s.get_info(RS2_CAMERA_INFO_NAME)
              << ". Supported options:" << std::endl;

    for (const auto& o : s.get_supported_options()) {
      std::cout << "\t" << rs2_option_to_string(o) << std::endl;
    }
  }
  auto device = context.query_devices()[0];

  std::cout << "Device " << device.get_info(RS2_CAMERA_INFO_NAME) << " connected" << std::endl;
  sensor = device.query_sensors()[0];

  if (manual_exposure) {
    std::cout << "Enabling manual exposure control" << std::endl;
    sensor.set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0);
    sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure_value * 1000);
  }
  // 
  std::cout << "finish open camera" << std::endl;
}

void RsD455Device::start() {

  auto callback = [&](const rs2::frame& frame) {


    if (auto fp = frame.as<rs2::motion_frame>()) {
      auto motion = frame.as<rs2::motion_frame>();
      if (motion && motion.get_profile().stream_type() == RS2_STREAM_GYRO &&
          motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F) {
            imu_num += 1;
      } 

      else if (motion &&
                 motion.get_profile().stream_type() == RS2_STREAM_ACCEL &&
                 motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F) {
        }
    } 
    else if (auto fs = frame.as<rs2::frameset>()) {
      std::vector<rs2::video_frame> vfs;
      for (int i = 0; i < NUM_CAMS; ++i) {
        rs2::video_frame vf = fs[i].as<rs2::video_frame>();
        if (!vf) {
          std::cout << "Weird Frame, skipping" << std::endl;
          return;
        }
        vfs.push_back(vf);
      }
      for (int i = 1; i < NUM_CAMS; ++i) {
        if (vfs[0].get_timestamp() != vfs[i].get_timestamp()) {
          return;
        }
      }
      if (frame_counter++ % skip_frames != 0) {
        return;
      }
      // 读取N个图像的数据
      for (int i = 0; i < NUM_CAMS; i++) {
        const auto& vf = vfs[i];

        int64_t t_ns = vf.get_timestamp() * 1e6;
        cv::Mat img = cv::Mat(cv::Size(640, 480), CV_8UC1, (void*)vfs[i].get_data());
        cv::imshow("img_"+std::to_string(i), img);

      }

      // 读取ir
      cv::waitKey(1);
      std::cout << "frame num = " << frame_num << " imu_num = " << imu_num << std::endl;
      frame_num += 1;
    }

    
    return;
  };

  
  profile = pipe.start(config, callback);
}


void RsD455Device::stop() {
  std::cout << "stop d455" << std::endl;
}

// 设置手动曝光参数
bool RsD455Device::setExposure(double exposure) {
  if (!manual_exposure) return false;
  sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure * 1000);
  return true;
}
// 设置跳过的帧数
void RsD455Device::setSkipFrames(int skip) { skip_frames = skip; }

void RsD455Device::setWebpQuality(int quality) { webp_quality = quality; }

int main(void){

  // realsense 设备
  RsD455Device d455_device(false, 1, 90, 10.0);
  
  // 开启设备和加载标定参数
  d455_device.start();// 开启相机

    while(1);
    return 0;
}